#include "pgObjects.h"
#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>

#define PiBy180 0.01745329251994329576923690768489F

//Physics Static Members definition
btVector3 pgObjects::p_worldAabbMin;
btVector3 pgObjects::p_worldAabbMax;
btBroadphaseInterface* pgObjects::p_broadPhase;
btDefaultCollisionConfiguration* pgObjects::p_collisionConfiguration;
btCollisionDispatcher* pgObjects::p_dispatcher;
btSequentialImpulseConstraintSolver* pgObjects::p_solver;
btDiscreteDynamicsWorld* pgObjects::p_dynamicsWorld;
btAlignedObjectArray<btCollisionShape*>	pgObjects::p_collisionShapes;

multimap<int,pgObjects*> pgObjects::pgObjectsMap;

void pgObjects::InitBulletPhysics(){
	p_worldAabbMin.setValue(-10000,-10000,-10000);
	p_worldAabbMax.setValue(10000,10000,10000);
	p_broadPhase = new btAxisSweep3 (p_worldAabbMin, p_worldAabbMax);

	p_collisionConfiguration = new btDefaultCollisionConfiguration();
	p_dispatcher = new btCollisionDispatcher(p_collisionConfiguration);
	p_solver = new btSequentialImpulseConstraintSolver;
	p_dynamicsWorld = new btDiscreteDynamicsWorld(p_dispatcher,p_broadPhase,p_solver,p_collisionConfiguration);
	p_dynamicsWorld->setGravity(btVector3(0,-10,0));
	p_dynamicsWorld->getDispatchInfo().m_useConvexConservativeDistanceUtil = true;
	p_dynamicsWorld->getDispatchInfo().m_convexConservativeDistanceThreshold = 0.01f;
}
void pgObjects::ExitBulletPhysics(){
	delete p_dynamicsWorld;
    delete p_solver;
    delete p_dispatcher;
    delete p_collisionConfiguration;
    delete p_broadPhase;
}
btRigidBody* pgObjects::localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape,int addToWorld){
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			shape->calculateLocalInertia(mass,localInertia);

		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
		
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);
		//don't add unconditinally; let the AI decide.
		//if(addToWorld==1){
			//p_dynamicsWorld->addRigidBody(body);
		//}

		return body;
	}

//Render Scene
multimap<int,pgObjects*>::iterator it;
pair< multimap<int,pgObjects*>::iterator,multimap<int,pgObjects*>::iterator > objList;

void pgObjects::renderScene(){
	unsigned int nObjects = pgObjectsMap.size();
	objList = pgObjectsMap.equal_range(TYPEID_SPACESHIP);

	for (it=objList.first; it!=objList.second; ++it)
	{
		it->second->drawGeometry(1);
	}
}

//what's this for?
pgObjects::pgObjects()
{
	yRot = 0.0f;
	isActive = false;
}

void pgObjects::setObjectPositionAndOrientation(float x1,float y1,float z1,float angle){
	position.setValue(x1,y1,z1);
	yRot = angle;	
	angle = angle * PiBy180 * 0.5f;

	//orient y-axis pointing upwards
	orientation.setRotation(btVector3(0,1,0),angle);
}